#include "MecanumChassis.h"
#include "DJ_Motor_Driver.h"
#include "BSP.h"
#include "JY931.h"
#include "FastMathFunctions.h"

// 底盘初始化
void Chassis_Init(void)
{
    AttachInterrupt_CAN(&hcan1, DJ_CAN_Callback);

    DJ_Init(1, M3508);
    DJ_Init(2, M3508);
    DJ_Init(3, M3508);
    DJ_Init(4, M3508);

    JY931_Init();

    chassis.roll = 0;
    chassis.pitch = 0;
    chassis.yaw = 0;

    /* 角度pid */
    PID_Init(&chassis.pid, 0.1, 0, 0, 3, 0);
}

/**
 * 设置底盘速度
 * 正反向为Y轴，右方向为X轴，m/s
 * 顺时针为正， rad/s
 */
void Chassis_SetSpeed(float Vx, float Vy, float Vw)
{
    /* 合速度大小 */
    float V = sqrtf(Vx * Vx + Vy * Vy);
    /* 合速度与Y轴夹角 */
    float angle = atan2f(Vx, Vy);

    JY931_GetAngle();
    PID_Calc(&chassis.pid, JY931.yaw, chassis.yaw);

    chassis.Motors_Speed[0] = (int16_t)((-V * fast_sin(angle) + V * fast_cos(angle) + (Vw - chassis.pid.out) * (LENGTH + WIDTH)) / PI / RADIUS * 60 * DECRATIO);
    chassis.Motors_Speed[1] = (int16_t)((-V * fast_sin(angle) - V * fast_cos(angle) + (Vw - chassis.pid.out) * (LENGTH + WIDTH)) / PI / RADIUS * 60 * DECRATIO);
    chassis.Motors_Speed[2] = (int16_t)((V * fast_sin(angle) - V * fast_cos(angle) + (Vw - chassis.pid.out) * (LENGTH + WIDTH)) / PI / RADIUS * 60 * DECRATIO);
    chassis.Motors_Speed[3] = (int16_t)((V * fast_sin(angle) + V * fast_cos(angle) + (Vw - chassis.pid.out) * (LENGTH + WIDTH)) / PI / RADIUS * 60 * DECRATIO);

    // chassis.Motors_Speed[0] = (int16_t)((-Vx + Vy + (Vw - chassis.pid.out) * (LENGTH + WIDTH)) / PI / RADIUS * 60 * DECRATIO);
    // chassis.Motors_Speed[1] = (int16_t)((-Vx - Vy + (Vw - chassis.pid.out) * (LENGTH + WIDTH)) / PI / RADIUS * 60 * DECRATIO);
    // chassis.Motors_Speed[2] = (int16_t)((Vx - Vy + (Vw - chassis.pid.out) * (LENGTH + WIDTH)) / PI / RADIUS * 60 * DECRATIO);
    // chassis.Motors_Speed[3] = (int16_t)((Vx + Vy + (Vw - chassis.pid.out) * (LENGTH + WIDTH)) / PI / RADIUS * 60 * DECRATIO);

    DJ_SetSpeed(chassis.Motors_Speed);
}

// 设置底盘角度
void Chassis_SetAngle(float angle)
{
}

// 设置底盘移动点
void Chassis_SetPoint(float Px, float Py, float angle)
{
}
